14. Implement a Cost Function in C++

In the previous quizzes, you designed a cost function to choose a lane when trying to reach a goal in highway driving:

Here, $\Delta d$ was the lateral distance between the goal lane and the final chosen lane, and $\Delta s$ was the longitudinal distance from the vehicle to the goal.

In this quiz, we'd like you to implement the cost function in C++, but with one important change. The finite state machine we use for vehicle behavior also includes states for planning a lane change right or left (PLCR or PLCL), and the cost function should incorporate this information. We will provide the following four inputs to the function:

• Intended lane: the intended lane for the given behavior. For PLCR, PLCL, LCR, and LCL, this would be the one lane over from the current lane.
• Final lane: the immediate resulting lane of the given behavior. For LCR and LCL, this would be one lane over.
• The $\Delta s$ distance to the goal.
• The goal lane.

Your task in the implementation will be to modify $|\Delta d|$ in the equation above so that it satisifes:

• $|\Delta d|$ is smaller as both intended lane and final lane are closer to the goal lane.
• The cost function provides different costs for each possible behavior: KL, PLCR/PLCL, LCR/LCL.
• The values produced by the cost function are in the range 0 to 1.

You can implement your solution in cost.cpp below.

Start Quiz:

#include <iostream>
#include "cost.h"

using std::cout;
using std::endl;

int main() {
  int goal_lane = 0;
    
  // Test cases used for grading - do not change.
  double cost;
  cout << "Costs for (intended_lane, final_lane, goal_distance):" << endl;
  cout << "---------------------------------------------------------" << endl;
  cost = goal_distance_cost(goal_lane, 2, 2, 1.0);
  cout << "The cost is " << cost << " for " << "(2, 2, 1.0)" << endl;
  cost = goal_distance_cost(goal_lane, 2, 2, 10.0);
  cout << "The cost is " << cost << " for " << "(2, 2, 10.0)" << endl;
  cost = goal_distance_cost(goal_lane, 2, 2, 100.0);
  cout << "The cost is " << cost << " for " << "(2, 2, 100.0)" << endl;
  cost = goal_distance_cost(goal_lane, 1, 2, 100.0);
  cout << "The cost is " << cost << " for " << "(1, 2, 100.0)" << endl;
  cost = goal_distance_cost(goal_lane, 1, 1, 100.0);
  cout << "The cost is " << cost << " for " << "(1, 1, 100.0)" << endl;
  cost = goal_distance_cost(goal_lane, 0, 1, 100.0);
  cout << "The cost is " << cost << " for " << "(0, 1, 100.0)" << endl;
  cost = goal_distance_cost(goal_lane, 0, 0, 100.0);
  cout << "The cost is " << cost << " for " << "(0, 0, 100.0)" << endl;
    
  return 0;
}

#include "cost.h"
#include <cmath>

double goal_distance_cost(int goal_lane, int intended_lane, int final_lane, 
                          double distance_to_goal) {
  // The cost increases with both the distance of intended lane from the goal
  //   and the distance of the final lane from the goal. The cost of being out 
  //   of the goal lane also becomes larger as the vehicle approaches the goal.
    
  /**
   * TODO: Replace cost = 0 with an appropriate cost function.
   */
  double cost = 0;
    
  return cost;
}

#ifndef COST_H
#define COST_H

double goal_distance_cost(int goal_lane, int intended_lane, int final_lane, 
                          double distance_to_goal);

#endif  // COST_H